using System;
using System.Collections.Generic;
using System.Text;
using MyMath;

namespace Physics
{
    public delegate bool CollisionHandler(CollisionParameters parameters);
    public class CollisionDetectorWC : ICollisionDetector
    {
        struct CollisionPair : IEquatable<CollisionPair>
        {
            public CollisionPair(IPhysicalObject a, IPhysicalObject b)
            {
                this.a = a;
                this.b = b;
            }
            IPhysicalObject a;
            IPhysicalObject b;

            #region IEquatable<CollisionPair> Members

            public bool Equals(CollisionPair other)
            {
                if ((a == other.a && b == other.b) || (b == other.a && a == other.b))
                    return true;
                else
                    return false;
            }

            #endregion
        }
        #region Fields
        List<IPhysicalObject> m_objects = new List<IPhysicalObject>();
        List<ConstrainingObject> constrainingObjects = new List<ConstrainingObject>();
        IntersectionQueue m_intersections = new IntersectionQueue();

        CollisionQueue m_collisions = new CollisionQueue();

        List<CollisionPair> resolvedCollisions = new List<CollisionPair>();

        RigidBodySimulator m_simulator;
        float currentTime;
        float endTime;

        //float m_collisionTime = 0.000f;
        //float m_collisionTimeTolerance = 0.001f;
        //float m_collisionTolerance = 0.1f;

        //int nf = 0;
        #endregion

        public event CollisionHandler OnPossibleCollision;
        public event CollisionHandler OnCollision;

        public CollisionDetectorWC(RigidBodySimulator simulator)
        {
            m_simulator = simulator;
        }

        public void AddObject(IPhysicalObject obj)
        {
            m_objects.Add(obj);
        }
        public void RemoveObject(IPhysicalObject obj)
        {
            m_objects.Remove(obj);
        }
        public void AddConstrainingObject(ConstrainingObject obj)
        {
            constrainingObjects.Add(obj);
        }
        public void RemoveConstrainingObject(ConstrainingObject obj)
        {
            constrainingObjects.Remove(obj);
        }

     

        private Collision CheckPossibleCollision(IPhysicalObject A, IPhysicalObject B)
        {
            if (A.Type == ObjectType.Static && B.Type == ObjectType.Static)
            {
                return null;
            }
            if (B.CollisionDataType == CollisionDataType.CollisionMesh && A.CollisionDataType != CollisionDataType.CollisionMesh)
            {
                IPhysicalObject temp = B;
                B = A;
                A = temp;
            }
            if (A.CollisionDataType == CollisionDataType.Point)
            {
                IPhysicalObject a = A;
                A = B;
                B = a;

            }
            Collision c = new Collision();

            c.Params = new CollisionParameters();

            bool status = false;
            float TimeOfCollision = 0;
            float dt;


            int timeShift;
            float shiftAmount;

            if (A.CurrentTime > B.CurrentTime)
            {
                timeShift = 1; //Move B in time back later
                shiftAmount = A.CurrentTime - B.CurrentTime;
                dt = endTime - A.CurrentTime;
                B.MoveToTime(A.CurrentTime);
            }
            else if (B.CurrentTime > A.CurrentTime)
            {
                timeShift = 2; //Move A in time back later
                shiftAmount = B.CurrentTime - A.CurrentTime;

                dt = endTime - B.CurrentTime;
                A.MoveToTime(B.CurrentTime);
            }
            else
            {
                timeShift = 0;
                shiftAmount = 0;
                dt = endTime - A.CurrentTime;
            }




            MyVector collisionPoint = new MyVector();
            MyVector collisionNormal = new MyVector();


            if (A.CollisionDataType == CollisionDataType.Point)
            {
                IPhysicalObject a = A;
                A = B;
                B = a;

            }
            if (A.CollisionDataType == CollisionDataType.Point)
            {
                if (timeShift == 1)
                {
                    B.Advance(-shiftAmount);
                }
                else if (timeShift == 2)
                {
                    A.Advance(-shiftAmount);
                }

                //no point to point collision
                return null;
            }

            //swap A and B for better performance
            //TODO

            if (A.CollisionDataType == CollisionDataType.CollisionSphere && B.CollisionDataType == CollisionDataType.CollisionMesh)
            {
                IPhysicalObject temp = B;
                B = A;
                A = temp;
            }
            if ((A.Position - B.Position).LengthSq > 2 * (A.BoundingSphereRadius + B.BoundingSphereRadius) * (A.BoundingSphereRadius + B.BoundingSphereRadius))
                return null;


            MyVector startPoint = A.PointToLocalCoords(B.Position);
            MyVector relativeVelocity = A.NormalToLocalCoords(B.Velocity - A.Velocity);

            MyVector endPoint = startPoint + relativeVelocity * dt;
            if (A.CollisionDataType == CollisionDataType.CollisionMesh)
            {
                MyVector axis = -A.AngularVelocity;
                axis.Normalize();
                MyQuaternion rot = MyQuaternion.FromAxisAngle((dt * A.AngularVelocity).Length, axis);
                endPoint.Rotate(rot);
            }
            else
            {

            }
            switch (B.CollisionDataType)
            {
                case CollisionDataType.Point:
                    status = A.CollisionData.IntersectPoint(out collisionPoint, out collisionNormal, out TimeOfCollision,
                        startPoint, endPoint, dt);
                    break;
                case CollisionDataType.CollisionPlane:
                    status = A.CollisionData.IntersectPlane(out collisionPoint, out collisionNormal, out TimeOfCollision,
                        startPoint, endPoint, dt, (CollisionPlane)((CollisionPlane)B.CollisionData.Clone()).Rotate((~A.Orientation) * B.Orientation));
                    break;
                case CollisionDataType.CollisionSphere:
                    status = A.CollisionData.IntersectSphere(out collisionPoint, out collisionNormal, out TimeOfCollision,
                        startPoint, endPoint, dt, (CollisionSphere)B.CollisionData);
                    break;
                case CollisionDataType.CollisionMesh:

                    CollisionMesh m = (CollisionMesh)B.CollisionData.Clone();
                    status = A.CollisionData.IntersectMesh(out collisionPoint, out collisionNormal, out TimeOfCollision,
                        startPoint, endPoint, dt, (CollisionMesh)(m).Rotate((~A.Orientation) * B.Orientation));
                    break;
                default:
                    status = false;
                    break;

            }

            

            if (TimeOfCollision < -3 * endTime)
                status = false;// 
            else if (TimeOfCollision < 0)
                /*status = false;//*/
                TimeOfCollision = 0;
            if (status == true && TimeOfCollision > dt)
                status = false;

            if (status == true)
            {

                c.A = A;
                c.B = B;
                c.Params.A = A;
                c.Params.B = B;
                c.T = TimeOfCollision + A.CurrentTime;

                c.Params.CollisionPoint = A.PointToGlobalCoords(collisionPoint);
                c.Params.CollisionNormal = A.NormalToGlobalCoords(collisionNormal);
                if (A.Type == ObjectType.Static && B.Type == ObjectType.Static)
                {
                    status = false;
                }
              
                else
                {
                  
                    MyVector v1, v2;
                    MyVector r1 = (c.Params.CollisionPoint - A.Position).Rotate(~A.Orientation);
                    v1 = A.Velocity +
                        (A.AngularVelocity ^ r1).Rotate(A.Orientation);



                    MyVector r2 = (c.Params.CollisionPoint - B.Position).Rotate(~B.Orientation);
                    v2 = B.Velocity +
                        (B.AngularVelocity ^ r2).Rotate(B.Orientation);


                    c.Params.RelativeVelocity = v1 - v2;

                }
               
                if (status)
                {

                    c.Params.CollisionTangent = -c.Params.RelativeVelocity + (c.Params.CollisionNormal * (c.Params.CollisionNormal * c.Params.RelativeVelocity));
                    c.Params.CollisionTangent.Normalize();
                    //unused for collisions of moving objects
                    c.Params.RelativeAcceleration = new MyVector(0, 0, 0);
                }
            }

            if (timeShift == 1)
            {
                B.Advance(-shiftAmount);
            }
            else if (timeShift == 2)
            {
                A.Advance(-shiftAmount);
            }

            if (status == true)
            {
                if (OnPossibleCollision != null)
                {
                    status = OnPossibleCollision(c.Params); //czy badac to zderzenie dalej?
                    if (status == true)
                        return c;
                    else
                        return null;
                }
                else
                {
                    return c;
                }

            }
            else
                return null;
        }

        public void ProcessFrame(float frameTime)
        {
            foreach (IPhysicalObject obj in this.m_objects)
            {
                obj.CurrentTime = 0;
            }

            constrainingObjects[0].PhysicalObject.CurrentTime = 0;

            //CollisionMesh mesh = (CollisionMesh)((CollisionMesh)m_objects[1].CollisionData.Clone()).Rotate(m_objects[0].Orientation);

            currentTime = 0;
            endTime = frameTime;
            //nf++;
            //if (nf > 132)
            //{
            //    nf--;
            //    nf++;
            //}


            for (int s = 0; s < m_objects.Count; s++)
            {
                for (int k = s + 1; k < m_objects.Count; k++)
                {
                    Collision c = CheckPossibleCollision(m_objects[s], m_objects[k]);
                    if (c != null && c.T < endTime && c.T >= currentTime)
                    {
                        //m_objects[s].Collisions++;
                        //m_objects[k].Collisions++;
                        m_collisions.Insert(c);
                    }
                }
            }

            while (m_collisions.Count > 0)
            {

                Collision c = m_collisions.PopHead();

                IPhysicalObject a, b;
                a = c.A;
                b = c.B;

                //a.Collisions--;
                //b.Collisions--;

                if (c.T > currentTime)
                    resolvedCollisions.Clear();
                else
                {
                    CollisionPair pair = new CollisionPair(a, b);
                    foreach (CollisionPair p in resolvedCollisions)
                    {
                        if (p.Equals(pair))
                        {
                            c = null;
                            break;

                        }
                    }
                }
                if (c != null)
                {
                    currentTime = c.T;

                    //perform collision between A and B
                    a.MoveToTime(c.T);
                    b.MoveToTime(c.T);

                    if (OnCollision != null)
                    {
                        bool status = OnCollision(c.Params); //czy badac to zderzenie dalej?     
                        if (status == false)
                            continue;
                    }


                    m_simulator.ResolveCollision(c.Params);

                    resolvedCollisions.Add(new CollisionPair(a, b));
                }
                IPhysicalObject temp = null;
                if (a.Type == ObjectType.Static)
                {
                    temp = a;
                    a = null;
                }
                if (b.Type == ObjectType.Static)
                {
                    temp = b;
                    b = null;
                }
                //remove collisions that contain a or b
                for (int j = 0; j < m_collisions.Count; j++)
                {
                    if (m_collisions[j].A == a || m_collisions[j].A == b ||
                        m_collisions[j].B == a || m_collisions[j].B == b)
                    {
                        //m_collisions[j].A.Collisions--;
                        //m_collisions[j].B.Collisions--;
                        m_collisions.Remove(j);
                        j--;

                    }
                }
                if (a == null)
                    a = temp;
                if (b == null)
                    b = temp;
                //find new possible intersections
                foreach (IPhysicalObject s in m_objects)
                {
                    if (s == a || s == b)
                    {
                        if (s != a && a != null)
                        {
                            c = CheckPossibleCollision(a, s);
                            if (c != null && c.T < endTime && c.T > currentTime)
                            {
                                m_collisions.Insert(c);
                                //c.A.Collisions++;
                                //c.B.Collisions++;
                            }
                        }
                        //if (s != b && b != null)
                        //{
                        //    c = CheckPossibleCollision(b, s);
                        //    if (c != null && c.T < endTime && c.T > currentTime)
                        //    {
                        //        m_collisions.Insert(c);
                        //        //c.A.Collisions++;
                        //        //c.B.Collisions++;
                        //    }
                        //}
                    }
                    else
                    {
                        if (a != null)
                        {
                            c = CheckPossibleCollision(a, s);
                            if (c != null && c.T < endTime && c.T >= currentTime)
                            {
                                if (c.T == currentTime)
                                {
                                    CollisionPair pair = new CollisionPair(a, s);
                                    foreach (CollisionPair p in resolvedCollisions)
                                    {
                                        if (p.Equals(pair))
                                        {
                                            c = null;
                                            break;

                                        }
                                    }
                                }
                                if (c != null)
                                    m_collisions.Insert(c);
                                //c.A.Collisions++;
                                //c.B.Collisions++;
                            }
                        }

                        if (b != null)
                        {
                            c = CheckPossibleCollision(b, s);
                            if (c != null && c.T < endTime && c.T >= currentTime)
                            {
                                if (c.T == currentTime)
                                {
                                    CollisionPair pair = new CollisionPair(b, s);
                                    foreach (CollisionPair p in resolvedCollisions)
                                    {
                                        if (p.Equals(pair))
                                        {
                                            c = null;
                                            break;

                                        }
                                    }
                                }
                                if (c != null)
                                    m_collisions.Insert(c);
                                //c.A.Collisions++;
                                //c.B.Collisions++;
                            }
                        }
                    }
                }
            }
            resolvedCollisions.Clear();
            //move all objects
            foreach (IPhysicalObject s in m_objects)
            {
                s.MoveToTime(endTime);
            }
            constrainingObjects[0].PhysicalObject.Advance(endTime);
            foreach (IPhysicalObject s in m_objects)
            {
                if (s.CollisionData != null && s!=constrainingObjects[0].PhysicalObject)
                {
                    Collision c = CheckConstraintCollision(this.constrainingObjects[0], s);
                    if (c != null)
                    {
                        this.m_simulator.ResolveCollision(c.Params);
                        s.Advance(c.T);
                    }
                }
            }
        }

        Collision CheckConstraintCollision(ConstrainingObject con, IPhysicalObject a)
        {
            
            Collision c = new Collision();

            c.Params = new CollisionParameters();

            IPhysicalObject b = con.PhysicalObject;

            float TimeOfCollision = a.CurrentTime;
            bool status = false;

            MyVector collisionPoint = new MyVector();
            MyVector collisionNormal = new MyVector();
            MyVector translateBack = new MyVector();
            switch (a.CollisionDataType)
            {
                case CollisionDataType.CollisionSphere:
                    status = con.IntersectSphere(b.PointToLocalCoords(a.Position), b.NormalToLocalCoords(a.Velocity), (CollisionSphere)a.CollisionData, out collisionPoint, out collisionNormal,out translateBack);
                    break;
                case CollisionDataType.CollisionMesh:
                    CollisionMesh m = (CollisionMesh)a.CollisionData.Clone();
                    m.Rotate(~b.Orientation*a.Orientation);
                    status = con.IntersectMesh(b.PointToLocalCoords(a.Position), b.NormalToLocalCoords(a.Velocity), m, out collisionPoint, out collisionNormal, out translateBack);
                    break;

            }

            

            if (status == true)
            {
                a.Position += b.PointToGlobalCoords(translateBack);
                c.A = con.PhysicalObject;
                c.B = a;
                c.Params.A = c.A;
                c.Params.B = c.B;
                if (a.Velocity.LengthSq > 0)
                    c.T = translateBack.Length / a.Velocity.Length;
                else
                    c.T = 0;

                c.Params.CollisionPoint = c.A.PointToGlobalCoords(collisionPoint);
                c.Params.CollisionNormal = c.A.NormalToGlobalCoords(collisionNormal);
                if (c.A.Type == ObjectType.Static && c.B.Type == ObjectType.Static)
                {
                    status = false;
                }
                else
                {

                    MyVector v1, v2;
                    MyVector r1 = (c.Params.CollisionPoint - c.A.Position).Rotate(~c.A.Orientation);
                    v1 = c.A.Velocity +
                        (c.A.AngularVelocity ^ r1).Rotate(c.A.Orientation);



                    MyVector r2 = (c.Params.CollisionPoint - c.B.Position).Rotate(~c.B.Orientation);
                    v2 = c.B.Velocity +
                        (c.B.AngularVelocity ^ r2).Rotate(c.B.Orientation);


                    c.Params.RelativeVelocity = v1 - v2;

                }

                if (status)
                {

                    c.Params.CollisionTangent = -c.Params.RelativeVelocity + (c.Params.CollisionNormal * (c.Params.CollisionNormal * c.Params.RelativeVelocity));
                    c.Params.CollisionTangent.Normalize();
                    //unused for collisions of moving objects
                    c.Params.RelativeAcceleration = new MyVector(0, 0, 0);
                }
            }



            if (status == true)
            {
                if (OnPossibleCollision != null)
                {
                    status = OnPossibleCollision(c.Params); //czy badac to zderzenie dalej?
                    if (status == true)
                        return c;
                    else
                        return null;
                }
                else
                {
                    return c;
                }

            }
            else
                return null;
        }



    }
}
